import numpy as np
import math
import sys
sys.path.append('../CWJ')
from Source.CWJ.gdpz import intersection_to_point, intersection_to_mian
from Source.CWJ.gdzbzh import pzjc
from Source.CWJ.qyhf import check

def qiudian2(pi, block, long):
    x = block[1][0] - block[0][1]
    y = block[1][1] - block[0][1]
    if x >= y:
        newpoint = [[pi[1][0], pi[1][1] - long, pi[1][2]], [pi[1][0], pi[1][1] + long, pi[1][2]]]
    else:
        newpoint = [[pi[1][0] - long, pi[1][1], pi[1][2]], [pi[1][0] + long, pi[1][1], pi[1][2]]]
    return newpoint

def qiudian(point1, point2, long, angle):
    p = np.zeros(2)
    for i in range(2):
        p[i] = point2[i] - point1[i]
    a = (p[0] ** 2 + p[1] ** 2) ** 0.5
    if p[0] != 0:
        k = p[1] / p[0]  # 直线1的斜率
    if angle == 1:  # 两直线垂直
        goal = np.zeros(3)
        goal1 = np.zeros(3)
        goal[0] = point2[0] + long * (p[1] / a)
        goal1[0] = point2[0] - long * (p[1] / a)
        goal[1] = point2[1] - long * (p[0] / a)
        goal1[1] = point2[1] + long * (p[0] / a)
        goal[2] = point2[2]
        goal1[2] = point2[2]
        # print(goal,goal1)
        return goal, goal1
    if angle == 2:  # 夹角为45度时
        if p[0] == 0:
            k = 0
        goal = np.zeros(3)
        goal1 = np.zeros(3)
        goal2 = np.zeros(3)
        goal3 = np.zeros(3)
        if k == 1 or k == -1:
            # 求出x，y坐标
            goal[0] = point2[0] - long
            goal[1] = point2[1]
            goal1[0] = point2[0] + long
            goal1[1] = point2[1]
            goal2[0] = point2[0]
            goal2[1] = point2[1] - long
            goal3[0] = point2[0]
            goal3[1] = point2[1] + long
        else:

            k1 = (1 + k) / (1 - k)
            a = (k1 ** 2 + 1) ** 0.5
            goal[0] = point2[0] + long * (1 / a)
            goal1[0] = point2[0] - long * (1 / a)
            goal[1] = point2[1] - long * (k1 / a)
            goal1[1] = point2[1] + long * (k1 / a)
            k2 = (k - 1) / (1 + k)
            b = (k2 ** 2 + 1) ** 0.5
            goal2[0] = point2[0] + long * (1 / b)
            goal3[0] = point2[0] - long * (1 / b)
            goal2[1] = point2[1] - long * (k2 / b)
            goal3[1] = point2[1] + long * (k2 / b)
        goal[2] = point2[2]
        goal1[2] = point2[2]
        goal2[2] = point2[2]
        goal3[2] = point2[2]
        # print(goal,goal1,goal2,goal3)
        return [goal, goal1, goal2, goal3]

def zuoyou(coor, stru):
    cx = []
    cy = []
    sx = []
    sy = []
    for i in range(0, len(coor)):
        cx.append(coor[i][0])
        cy.append(coor[i][1])
    for j in range(0, len(stru)):
        sx.append(stru[j][0])
        sy.append(stru[j][1])
    cmax = [max(cx), max(cy)]
    smax = [max(sx), max(sy)]
    cmin = [min(cx), min(cy)]
    smin = [min(sx), min(sy)]
    dis1 = math.sqrt((cmax[0] - smax[0]) ** 2 + (cmax[1] - smax[1]) ** 2)
    dis2 = math.sqrt((cmin[0] - smin[0]) ** 2 + (cmin[1] - smin[1]) ** 2)
    return dis2 - dis1

# 输入碰撞坐标和管道流，输出碰撞的平均坐标
def Average_value(coor, pi):
    index = len(coor)
    x = 0
    y = 0
    z = 0
    for i in range(index):
        x = x + coor[i][0]
        y = y + coor[i][1]
        z = z + coor[i][2]
    x = x / index
    y = y / index
    z = z / index
    xyz = [x, y, z]
    redis = pi[3] / 2  # 回退的长度为半个障碍物的宽度
    step = [redis / x, redis / y, redis / z]  # 步长
    for j in range(0, 3):
        if pi[1][j] - pi[0][j] == 0:
            xyz[j] = pi[0][j]
        else:
            if pi[1][j] >= pi[0][j]:
                xyz[j] = (xyz[j] * (1 - step[j]))
            else:
                xyz[j] = (xyz[j] * (1 + step[j]))
    xy = [x - xyz[0], y - xyz[1]]
    return [xyz, xy]


# 上下型障碍物
# 输入管道流和碰撞坐标，输出避障后的管道流
# pi: 管道流
# stru: 上下型障碍物表达式（与管道表示相同
# temp: 上下型障碍物的类型
# list1: 附近障碍物的列表
def Avoid_crossbeam(pi, stru, block_list, temp, BlockBase):
    z1 = pi[1][2]
    coor = intersection_to_point(pi, stru)  # 碰撞坐标
    m = 0
    xyz = Average_value(coor, pi)
    high = stru[3]  # 障碍物的高度
    # lenp：管道回退的距离
    # d1: 回退的距离
    lenp = math.sqrt((xyz[0][0] - pi[0][0]) ** 2 + (xyz[0][1] - pi[0][1]) ** 2)
    d1 = math.sqrt(xyz[1][0] ** 2 + xyz[1][1] ** 2)
    # 用管道回退后剩下的长度除以回退的距离，计算出可以循环的次数
    if d1 == 0:
        final_pi = pi
    else:
        j1 = int(lenp / d1)
        if temp == 'Frame':  # 如果是横梁，向下
            for j in range(0, j1 + 1):
                new_pi = [pi[0], xyz[0], pi[2], pi[3]]  # 回退后的管道流
                for i in range(1, 9):
                    new_xyz = [new_pi[1][0], new_pi[1][1], new_pi[1][2] - 0.25 * i * high]
                    pi2 = [new_pi[1], new_xyz, pi[2], pi[3]]  # 向下的直管
                    a = pzjc([pi2], [stru])
                    if a == [[]]:
                        pi3 = [pi2[1], [pi[1][0], pi[1][1], pi2[1][2]], pi[2], pi[3]]  # 直管接下的横管（与障碍物平行）
                        b = pzjc([pi3], [stru])
                        if b == [[]]:
                            final_pi = [new_pi, pi2, pi3]
                            m = 1
                            for z in range(0, len(block_list)):
                                d = pzjc(final_pi, [block_list[z]])
                                e = []
                                for o in range(0, len(final_pi)):
                                    e.append([])
                                if d != e:
                                    final_pi = Pi_bypass(final_pi, block_list, BlockBase)
                            f = pzjc(final_pi, block_list)
                            if len(block_list) == 0:
                                break
                            else:
                                g = []
                                for o in range(0, len(final_pi)):
                                    g.append([])
                                if f == g:  # 绕行完成
                                    break
                        if i == 8:
                            m = 2
                            final_pi = pi
                            break
                    else:
                        xyz1 = Average_value([xyz[0]], pi)
                        xyz[0] = xyz1[0]
                        if j == j1:
                            final_pi = pi
                        break

                if m == 1 or m == 2 or m == 3:
                    break

        else:  # 否则是管道或其他
            # 向上
            for j in range(0, j1 + 1):
                new_pi = [pi[0], xyz[0], pi[2], pi[3]]  # 回退后的管道流
                for i in range(1, 9):
                    new_xyz = [new_pi[1][0], new_pi[1][1], new_pi[1][2] + 0.25 * i * high]
                    height = 0.25 * i * high
                    pi2 = [new_pi[1], new_xyz, pi[2], pi[3]]  # 向上的直管
                    a = pzjc([pi2], [stru])
                    if a == [[]]:
                        pi3 = [pi2[1], [pi[1][0], pi[1][1], pi2[1][2]], pi[2], pi[3]]  # 直管接下的横管（与障碍物平行）
                        b = pzjc([pi3], [stru])
                        if b == [[]]:
                            final_pi = [new_pi, pi2, pi3]
                            m = 1
                            for z in range(0, len(block_list)):
                                d = pzjc(final_pi, [block_list[z]])
                                e = []
                                for o in range(0, len(final_pi)):
                                    e.append([])
                                if d != e:
                                    final_pi = Pi_bypass(final_pi, block_list, BlockBase)
                            f = pzjc(final_pi, block_list)
                            if len(block_list) == 0:
                                break
                            else:
                                g = []
                                for o in range(0, len(final_pi)):
                                    g.append([])
                                if f == g:  # 绕行完成
                                    break
                        if i == 8:
                            m = 2
                            final_pi = Avoid_crossbeam(pi, stru, block_list, 'Frame', BlockBase)
                            final_pi = final_pi[0]
                            break
                    else:
                        xyz1 = Average_value([xyz[0]], pi)
                        xyz[0] = xyz1[0]
                        if j == j1:
                            final_pi = pi
                        break

                if m == 1 or m == 2 or m == 3:
                    break
    len_finalpi = len(final_pi)
    z2 = final_pi[len_finalpi - 1][1][2]
    rectify = [0, 0, z2 - z1]
    return [final_pi, rectify]

# 左右型障碍物
# 输入管道流，碰撞坐标和障碍物类型，输出避障后的管道流
# coor：碰撞点坐标
# pi: 管道流
# column: 上下型障碍物表达式（与管道流表达相同
# stru: 障碍物产生碰撞的面上的四点坐标
def Avoid_column(pi, column, block_list, BlockBase, temp=0):
    coor = intersection_to_point(pi, column)
    stru = intersection_to_mian(pi, column)
    index = 0
    m = 0
    n = 0
    wide = max(column[2], column[3])  # 障碍物宽度
    xyz = Average_value(coor, pi)
    # 管道终点 距离 碰撞平均坐标的长度
    lenp = math.sqrt((xyz[0][0] - pi[0][0]) ** 2 + (xyz[0][1] - pi[0][1]) ** 2)
    d1 = math.sqrt(xyz[1][0] ** 2 + xyz[1][1] ** 2)
    # 用管道回退后剩下的长度除以回退的距离，计算出可以循环的次数
    if d1 != 0:
        j1 = int(lenp / d1)
        if temp == 0:
            dis = zuoyou(coor, stru)
        else:
            dis = temp
            # #point1管道1起点，point2管道1终点，long管道2长度，angle夹角angle=1为垂直，angle=2为45度夹角

        if dis >= 0:  # 向右拐
            # ①
            for j in range(0, j1 + 1):
                len_pi = math.sqrt((pi[1][0] - xyz[0][0]) ** 2 + (pi[1][1] - xyz[0][1]) ** 2)  # 管道终点到回退点的距离
                new_pi = [pi[0], xyz[0], pi[2], pi[3]]  # 回退后的管道流
                # 横向增加的长度取两个wide
                # ②
                for i in range(1, 9):
                    a = qiudian(new_pi[0], new_pi[1], (i / 4) * wide, 1)
                    list11 = a[0].tolist()
                    pi2 = [new_pi[1], list11, pi[2], pi[3]]  # 向右拐（y方向）
                    test1 = pzjc([pi2], [column])
                    # ③
                    if test1 == [[]]:
                        b = qiudian(pi2[0], pi2[1], len_pi, 1)
                        list2 = b[1].tolist()
                        pi3 = [pi2[1], list2, pi[2], pi[3]]
                        a1 = pzjc([pi3], [column])
                        # ④
                        if a1 == [[]]:
                            m = 1
                            final_pi = [new_pi, pi2, pi3]
                            # 遍历附近所有障碍物
                            # ⑤
                            for z in range(0, len(block_list)):
                                d = pzjc(final_pi, [block_list[z]])
                                if d == [[], [], []]:
                                    n = n + 1
                                else:
                                    continue
                            if n == len(block_list):  # 绕行完成
                                break
                            else:  # 向右绕行有障碍物，尝试向左
                                m = 3
                                if temp == 0:    # 第一次调用，尝试向左
                                    final_pi = Avoid_column(pi, column, block_list, BlockBase, -1)
                                    f = pzjc(final_pi, block_list)
                                    g = []
                                    for o in range(0, len(final_pi)):
                                        g.append([])
                                    if f == g:
                                        break
                                else:            # 第n次调用，重新计算避障
                                    final_pi = Pi_bypass(final_pi, block_list, BlockBase)
                        # ④
                        else:
                            if i == 8:
                                m = 2
                                index = 1
                            continue
                    # ③
                    else:
                        xyz1 = Average_value([xyz[0]], pi)
                        xyz[0] = xyz1[0]
                        if j == j1:
                            index = 1
                        break
                # 若m = 1， 说明已经完成了绕行，直接退出①
                # 若m = 2， 说明pi2可以绕过，pi3不行，回退也无法绕行，直接退出①
                # 若m = 3， 说明向右绕行成功，但是会撞上其他障碍物，直接退出①
                if m == 1 or m == 2 or m == 3:
                    break
        else:  # 向左
            for j in range(0, j1 + 1):
                len_pi = math.sqrt((pi[1][0] - xyz[0][0]) ** 2 + (pi[1][1] - xyz[0][1]) ** 2)
                new_pi = [pi[0], xyz[0], pi[2], pi[3]]  # 回退后的管道流
                for i in range(1, 9):
                    a = qiudian(new_pi[0], new_pi[1], (i / 4) * wide, 1)
                    list11 = a[1].tolist()
                    pi2 = [new_pi[1], list11, pi[2], pi[3]]  # 向右拐（y方向）
                    test1 = pzjc([pi2], [column])
                    if test1 == [[]]:
                        b = qiudian(pi2[0], pi2[1], len_pi, 1)
                        list2 = b[0].tolist()
                        pi3 = [pi2[1], list2, pi[2], pi[3]]
                        a1 = pzjc([pi3], [column])
                        if a1 == [[]]:
                            m = 1
                            final_pi = [new_pi, pi2, pi3]
                            for z in range(0, len(block_list)):
                                d = pzjc(final_pi, [block_list[z]])
                                if d == [[], [], []]:
                                    n = n + 1
                                else:
                                    continue
                            if n == len(block_list):  # 绕行完成
                                break
                            else:  # 向右绕行有障碍物
                                m = 3
                                if temp == 0:  # 第一次调用，尝试向右
                                    final_pi = Avoid_column(pi, column, block_list, BlockBase, 1)
                                    f = pzjc(final_pi, block_list)
                                    g = []
                                    for o in range(0, len(final_pi)):
                                        g.append([])
                                    if f == g:
                                        break
                                else:  # 第n次调用，重新计算避障
                                    final_pi = Pi_bypass(final_pi, block_list, BlockBase)
                                # else:
                                #     index = 1
                                # break
                        else:
                            if i == 8:
                                m = 2
                                index = 1
                            continue
                    else:
                        xyz1 = Average_value([xyz[0]], pi)
                        xyz[0] = xyz1[0]
                        if j == j1:
                            index = 1
                        break
                if m == 1 or m == 2 or m == 3:
                    break

            # if m == 3 and temp == 0:
            #     final_pi = Avoid_column(pi, column, block_list, 1)
    else:
        index = 1
    if index == 1:  # 左右都有障碍物 从中穿过
        final_pi = pi

    return final_pi


# temp: 障碍物的属性
# pi： 管道坐标
# stru: 障碍物表达式
# list1: 附近障碍物列表
# def Avoid_obstacles(pi, stru, block_list, temp, BlockBase):
#     if temp == 'Frame':
#         new_pi = Avoid_crossbeam(pi, stru, block_list, temp, BlockBase)
#     elif temp == 'Column':
#         new_pi = Avoid_column(pi, stru, block_list, BlockBase)
#     elif temp == 'Pipe' or 'None':
#         if abs(stru[1][2] - stru[0][2]) < 50:
#             new_pi = Avoid_crossbeam(pi, stru, block_list, temp, BlockBase)
#         else:
#             new_pi = Avoid_column(pi, stru, block_list, BlockBase)
#     else:
#         new_pi = pi
#     return new_pi

def Avoid_obstacles(pi, stru, block_list, temp):
    # if abs(stru[1][2] - stru[0][2]) > 50:    # 左右型避障
    #     new_pi = Avoid_column(pi, stru, block_list)
    # else:
    #     if abs(pi[1][2] - pi[0][2]) > 50:
    #             new_pi = Avoid_column(pi, stru, block_list)
    #     else:
    #         new_pi = Avoid_crossbeam(pi, stru, block_list)
    if temp == 'Frame':
        new_pi1 = Avoid_crossbeam(pi, stru, block_list, temp)
        new_pi = new_pi1[0]
        rectify = new_pi1[1]
        # print(new_pi)
        # print(rectify)
    elif temp == 'Column':
        new_pi1 = Avoid_column(pi, stru, block_list)
        new_pi = new_pi1[0]
        rectify = new_pi1[1]
    elif temp == 'Pipe':
        if abs(stru[1][2] - stru[0][2]) < 50:
            if abs(pi[1][2] - pi[0][2]) > 50:
                new_pi1 = Avoid_column(pi, stru, block_list)
                new_pi = new_pi1[0]
                rectify = new_pi1[1]
            else:
                new_pi1 = Avoid_crossbeam(pi, stru, block_list, temp)
                new_pi = new_pi1[0]
                rectify = new_pi1[1]
        else:
            new_pi1 = Avoid_column(pi, stru, block_list)
            new_pi = new_pi1[0]
            rectify = new_pi1[1]
    else:
        new_pi = pi
        rectify = [0, 0, 0]
    return [new_pi, rectify]

# 输入管道，障碍物列表，障碍物类型，输出避障后的管道
# 需完善：temp为障碍物类型，可考虑在函数中获取障碍物的类型，而不作为参数使用
def Pi_bypass(pi, block_list, BlockBase):
    # x = len(pi)
    if not block_list:
        print("no blocks")
        return pi
    for j in range(0, len(pi)):
        for i in range(0, len(block_list)):
            a = pzjc([pi[j]], [block_list[i]])
            if a != [[]]:
                type = check([block_list[i]], BlockBase)
                block_list_leave = []
                for z in range(1, len(block_list)):
                    block_list_leave.append(block_list[z])
                pi1 = Avoid_obstacles(pi[j], block_list[i], block_list_leave, type, BlockBase)
                new_pi = pi1[0]
                rectify = pi1[1]
                if new_pi != pi[j]:
                    pi.pop(j)
                    for z in range(0, len(new_pi)):
                        pi.insert(j + z, new_pi[z])
                    for o in range(len(new_pi), len(pi)):
                        pi[o][0][0] = pi[o][0][0] + rectify[0]
                        pi[o][0][1] = pi[o][0][1] + rectify[1]
                        pi[o][0][2] = pi[o][0][2] + rectify[2]
                        pi[o][1][0] = pi[o][1][0] + rectify[0]
                        pi[o][1][1] = pi[o][1][1] + rectify[1]
                        pi[o][1][2] = pi[o][1][2] + rectify[2]
                    break
                else:
                    break

            else:
                continue
    return pi

# list1 = [[[348593.065319341, 120641.158927369, 56900.0], [348593.065319341, 120641.158927369, 55568.9], 1100.0, 100.0], [[349243.865319341, 120641.158927369, 54918.1], [350693.065319341, 120641.158927369, 54918.1], 1200.0, 100.0]]
# pi = [[[350216.954417915, 120562.224099402, 56121.2], [347416.954417915, 120562.224099402, 56168.8], 100.0, 100.0]]
# newpi = Pi_bypass(pi, list1, BlockBase)


